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Планирование траектории перемещения рабочего органа в декартовой системе координат приводит к необходимости сглажи- 
вания дискретной последовательности узлов траектории. Классический метод наименьших квадратов не применим в силу того, 
что при сглаживании начальное и конечное положение рабочего органа робота-манипулятора должны быть строго определе- 
ны. Для решения этой задачи предлагается модифицированный метод наименьших квадратов. Этот метод позволяет построить 
сглаженную траекторию перемещения рабочего органа с сохранением его начального и конечного положения; уменьшить ди- 
намическую нагрузку на приводы и звенья робота-манипулятора. 


Введение 

Самостоятельное выполнение технологической 
операции простейшими программируемыми робо- 
тами-манипуляторами невозможно без предвари- 
тельного построения функций управления привода- 
ми, совместное выполнение которых обеспечит вы- 
полнение заданной операции. Основой построения 
функций управления приводами является дискрет- 
ный план траектории перемещения рабочего органа 
(РО) робота-манипулятора [1]. Перемещение РО по 
дискретным точкам и в особенности движение во 
внешней среде с препятствиями приводит к необхо- 
димости резкого изменения направления его движе- 
ния. В силу этого возникают большие динамические 
нагрузки на звенья манипулятора, что приводит к 
более быстрому износу механизма и увеличивает по- 
требление электроэнергии. Решение этой задачи за- 
ключается в сглаживании дискретного плана траек- 
тории так, чтобы уменьшить динамические нагруз- 
ки, возникающие при перемещении РО из началь- 
ного положения в целевое. 

Постановка задачи 

Дана сетка, по узлам которой происходило пла- 
нирование траектории (г - расстояние между со- 
седними узлами сетки) и последовательность из п 
узловых точек спланированной (исходной) траек- 
тории в 3-мерном пространстве: 

(•*), У,, г), г = 0, ...,(« -1), (1) 

где і — порядковый номер узла. 

Необходимо заменить последовательность (1), 
на последовательность узловых точек: 

(х\, У', А А і = 0,...,(и— 1), 

таким образом, чтобы первая и последняя точки 
траектории, после сглаживания остались прежни- 
ми, т. е. выполнялось следующее условие: 

Х 0 - Х 0’ У О = У О ’ 2 0 = А) > 

•Сі =*„-!> У'„-1=Уп-1’ <і=Ѵі- (2) 

Кроме того, необходимо, чтобы новая (сгла- 
женная) траектория была не только максимально 
гладкой, но и достаточно близкой к исходной тра- 
ектории для обхода всех препятствий. 


Решение задачи 

Согласно методу наименьших квадратов [2] 
сглаживание траектории происходит путем её за- 
мены на полином Р т (і): 

т 

А=2ф(о=2>ѵ'"'*, (3) 

к = 0 

где т - порядок полинома, к - номер коэффици- 
ента полинома, ах к - коэффициенты полинома, 
которые находятся из системы линейных алгебраи- 
ческих уравнений (СЛАУ): 


Х[(^„(0-уГ-Д = 0, 7=0 (4) 


где у - номер уравнения в системе. 

Здесь и далее формулы для координат у и і ана- 
логичные. 

Условие (2) с использованием (3) можно запи- 
сать следующим образом: 

х о=Р т (0), х п _ х = Р т (п-\). (5) 

При условии (5) систему (4) можно преобразо- 
вать к виду: 


\^[(Р т (і)-х і )Г']=0, 7=0,...,»;. (6) 


Для того чтобы после сглаживания выполня- 
лось условие (2), необходимо заменить любые два 
уравнения в (6) уравнениями (5). Например, после 
замены »г-ого и (м-І)-ого уравнения на уравнения 
(5), СЛАУ примет вид: 


2[(С (0-х,)Г '] = 0, у' = 0,...,(»;-2); 

1=1 


Г т ( п -\)-х п _ ] =0, (т-1); 

Р т (0)-х (> =0, т. (7) 
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СЛАУ (7) в матричной форме: 

ВхАх = Сх, (8) 

где Вх - матрица СЛАУ, Сх - вектор свободных 
членов СЛАУ, Ах - вектор искомых коэффициен- 
тов полинома (3). 

Максимально допустимое отклонение от исход- 
ной траектории не должно превышать половины от 
длины диагонали клетки: 0,5л/3 г. Этот критерий 
можно использовать для автоматического подбора 
необходимой степени сглаживающего полинома. 


узлами не меньше расстояния от узла исходной тра- 
ектории до сглаженной траектории. Кроме того, рас- 
стояние между двумя точками в пространстве найти 
значительно легче и быстрее, чем найти расстояние 
от узла исходной траектории до сглаженной траекто- 
рии. В результате такой замены ужесточается крите- 
рий максимально допустимого отклонения и умень- 
шается время поиска этой оценки. Ужесточение кри- 
терия может привести к выбору более высокой сте- 
пени сглаживающего полинома и к сближению сгла- 
женной траектории к исходной траектории. 


Описание алгоритма сглаживания 

Вход: последовательность точек в 3-мерном 
пространстве, представляющих план траектории 
(х„ у„ $), размер сетки планирования г. 

Выход: функция//) сглаженной траектории. 

1 . Степень сглаживающего полинома т:= 1 . 

2. Цикл. 

2.1. ПоискУ, У, /-составляющих траектории: 
формирование и решение системы уравне- 
ний (8). 

2.2. Если максимальное отклонение <0,5л/3 г, 
то переход к шагу 3. 

2.3. Увеличение степени сглаживающего поли- 
нома т:=т+ 1. 

2.4. Переход к шагу 2.1. 

3. Конец. 

Система уравнений (8) строится по формулам: 


Вх і, к = 


Сх. = 


^і 2тЧ ~ к , 7=0,..., (т - 2), к=0,...,т; 

п т ~ к , ]=т-\, к=0,...,т; , 

0, ) = т, к = 0, ...,(»; -1); 

1, 7 = т, к = т, 

X •х/"-'' > 7 = 0,. ..,{т -2); 


*о> 

х„ , 


І = т-1; 
7 = т, 


После нахождения всех составляющих траекто- 
рии оценивается максимальное отклонение сгла- 
женной траектории от исходной траектории (шаг 
2.2 алгоритма сглаживания). Это происходит сле- 
дующим образом: 

1. Сглаженная траектория разбивается на после- 
довательность узлов равномерно по времени. 
Количество узлов равно количеству узлов ис- 
ходной траектории. 

2. Ищется максимальное расстояние между соот- 
ветствующими узлами исходной и сглаженной 
траекторий. 

Результат такого алгоритма не может быть зани- 
женным, т. к. расстояние между соответствующими 


Результаты работы 

В среде программирования БефЫ [3] написана и 
встроена в систему РАУМС [4] подпрограмма сгла- 
живания траектории. Был проведен эксперимент по 
моделированию движения гипотетического меха- 
низма (робота-манипулятора) в некой рабочей среде 
с препятствием (рис. 1). Механизм имеет три кине- 
матических узла: первый (основной) вращает осно- 
вание манипулятора, представленного в виде усечен- 
ного конуса; второй выдвигает балку, лежащую на 
основании; третий - вертикально перемещает РО. 

Система спланировала траекторию (рис. 2, 
пунктир), подпрограмма сглаживания выполнила 
сглаживание траектории (рис. 2, сплошная кри- 
вая). Подпрограмма автоматически подобрала сте- 
пень сглаживающего полинома т=А. Из рисунка 
видно, что начальные и конечные точки траекто- 
рий совпадают. 

Для реализации механизмом не сглаженной 
траектории на основной кинематический узел 
необходимо воздействовать силой (рис. 3, а), абсо- 
лютное значение которой достигает 100 Н. При ре- 
ализации сглаженной траектории (рис. 3, б) абсо- 
лютное значение силы не превышает 30 Н и дости- 
гает примерно 75 Н только на конечном участке. 
Ненулевые значения сил на границах траектории 
обусловлены разгоном в начале движения и тормо- 
жением в конце. Таким образом, усилия, необходи- 
мые для движения механизма по сглаженной тра- 
ектории, гораздо меньше усилий, необходимых для 
движения механизма по исходной траектории. 

Заключение 

Предложена модификация метода наименьших 
квадратов для сглаживания дискретного плана тра- 
ектории рабочего органа робота-манипулятора. 
Метод реализован в среде программирования 
Оеірііі |3| в виде подпрограммы и встроен в систе- 
му РАУМС [4]. 

Данная подпрограмма позволяет: 

1) построить сглаженную траекторию, для отработ- 
ки которой требуется меньше усилий, причем 
время прохождения траектории не увеличивается, 

2) значительно снизить динамические нагрузки, 
возникающие в звеньях манипулятора, повы- 
сить длительность эксплуатации и сократить 
затраты электроэнергии. 


177 




Известия Томского политехнического университета. 2006. Т. 309. № 8 



Файл Параметры Помощь 


конечная 
позиция РО 


рабочий 
орган (РО) 


начальная 
позиция РО 


Всего кадров : 80 


Ікорость: 5 


Рис. 1. Вид рабочей сцены 


Рис. 2. Виды исходной и сглаженной траекторий 


Фа\п Пгрз.стр Сгргвю 


а б 

Рис. 3. Г оафики сил, прилагаемых к основному кинематическому узлу от времени: а) без сглаживания; б) после сглаживания 
полиномом 4-го порядка 
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По сравнению с классическим методом наиме- 
ньших квадратов, данный алгоритм обеспечивает 
совпадение начальных и конечных точек траекто- 
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